#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <cvctracker/cvc_tracker.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>
#include <ctype.h>
#define FREEGLUT_STATIC
#include <freeglut/GL/freeglut.h>

using namespace cv;
using namespace std;

enum detecFeature{CHESSBOARD, NATURAL};
bool isNaturalAvalible = false;
int detecFeatureMode = CHESSBOARD;
bool enhancedMatching = false;
Mat img_matches;

VideoCapture cap;
Mat frame;
Mat gray, prevGray, image;

double proj[16];
double mv[16];
double mvInv[16];
double cameraPos[3];
double cameraRot[9];

cv::SurfFeatureDetector detector(600);
cv::SurfDescriptorExtractor extractor;
cv::DescriptorMatcher *matcher  = new cv::BFMatcher(cv::NORM_L2,true);
CVCTracker cvct(&detector , &extractor , matcher);
bool realtime = true;

double _radius = 700, _latitude = 89, _azimute=00;


void printmat(cv::Mat M)
{
	for(int i = 0; i < M.rows; i++)
	{
		const double* Mi = M.ptr<double>(i);
		for(int j = 0; j < M.cols; j++)
			printf(" %lf ",Mi[j]);
		printf("\n");
	}
	printf("\n");printf("\n");
}


void renderCube(float size)
{
	glBegin(GL_QUADS);
	// Front Face
	glNormal3f( 0.0f, 0.0f, 1.0f);
	glVertex3f( 0.0f,  0.0f,  0.0f);
	glVertex3f( size,  0.0f,  0.0f);
	glVertex3f( size,  size,  0.0f);
	glVertex3f( 0.0f,  size,  0.0f);
	// Back Face
	glNormal3f( 0.0f, 0.0f,-1.0f);
	glVertex3f( 0.0f,  0.0f, size);
	glVertex3f( 0.0f,  size, size);
	glVertex3f( size,  size, size);
	glVertex3f( size,  0.0f, size);		
	// Top Face
	glNormal3f( 0.0f, 1.0f, 0.0f);
	glVertex3f( 0.0f,  size,  0.0f);
	glVertex3f( size,  size,  0.0f);
	glVertex3f( size,  size, size);
	glVertex3f( 0.0f,  size, size);
	// Bottom Face
	glNormal3f( 0.0f,-1.0f, 0.0f);
	glVertex3f( 0.0f,  0.0f,  0.0f);
	glVertex3f( 0.0f,  0.0f, size);
	glVertex3f( size,  0.0f, size);
	glVertex3f( size,  0.0f,  0.0f);
	// Right face
	glNormal3f( 1.0f, 0.0f, 0.0f);
	glVertex3f( size,  0.0f, 0.0f);
	glVertex3f( size,  0.0f, size);
	glVertex3f( size,  size, size);
	glVertex3f( size,  size, 0.0f);
	// Left Face
	glNormal3f(-1.0f, 0.0f, 0.0f);
	glVertex3f( 0.0f,  0.0f, 0.0f);
	glVertex3f( 0.0f,  size, 0.0f);
	glVertex3f( 0.0f,  size, size);
	glVertex3f( 0.0f,  0.0f, size);
	glEnd();
}

void renderAxis(float size)
{
	glBegin(GL_LINES);
	glColor3f(1.0,0.0,0.0);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(size, 0.0f, 0.0f);
	glColor3f(0.0,1.0,0.0);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, size, 0.0f);
	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, size);
	glEnd();
}

void glGetModelViewMatrix( cv::Mat&  Rvec ,cv::Mat& Tvec, double modelview_matrix[16])
{

	//coordinate system returned is relative to the first 3D input point	
	for (int f=0; f<3; f++)
	{
		for (int c=0; c<3; c++)
		{
			modelview_matrix[c*4+f] = Rvec.ptr<double>()[f*3+c];	//transposed
			if (f>0)
			{
				modelview_matrix[c*4+f] *=-1;
			}
		}
	}	

	
	modelview_matrix[3] = 0.0;
	modelview_matrix[7] = 0.0;	
	modelview_matrix[11] = 0.0;
	modelview_matrix[12] = Tvec.at<double>(0,0);
	modelview_matrix[13] = -Tvec.at<double>(1,0);
	modelview_matrix[14] = -Tvec.at<double>(2,0);
	modelview_matrix[15] = 1.0;	


}

void getCameraPosition(double m[16],double mI[16],double trans[3],double rot[9])
{
	cv::Mat modelview = (cv::Mat_<double>(4,4) <<   m[0],m[1],m[2],m[3],   m[4],m[5],m[6],m[7],   m[8],m[9],m[10],m[11],   m[12],m[13],m[14],m[15]);
	cv::Mat modelviewinv = modelview.inv();
	
	memcpy(mI,modelviewinv.ptr<double>(),sizeof(double)*16);
	//TODO retirar ate o final da funcao
	printf("\n m \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n",m[0],m[1],m[2],m[3],m[4],m[5],m[6],m[7],m[8],m[9],m[10],m[11],m[12],m[13],m[14],m[15]);
	printf("\n mI \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n",mI[0],mI[1],mI[2],mI[3],mI[4],mI[5],mI[6],mI[7],mI[8],mI[9],mI[10],mI[11],mI[12],mI[13],mI[14],mI[15]);
	//printf(" %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n",ptr[0],ptr[1],ptr[2],ptr[3],ptr[4],ptr[5],ptr[6],ptr[7],ptr[8],ptr[9],ptr[10],ptr[11],ptr[12],ptr[13],ptr[14],ptr[15]);

	printf("\nModelView\n");
	printmat(modelview);
		
	printf("\nModelView invertida\n");
	printmat(modelviewinv);
	printf("%lf %lf %lf\n", modelviewinv.at<double>(3,0), modelviewinv.at<double>(3,1), modelviewinv.at<double>(3,2));
	trans[0] = modelviewinv.at<double>(3,0);
	trans[1] = modelviewinv.at<double>(3,1);
	trans[2] = modelviewinv.at<double>(3,2);
	
	//rotacao da camera em coordenadas do mundo por linhas
	rot[0] = m[0]; rot[3] = m[4]; rot[6] = m[8];
	rot[1] = m[1]; rot[4] = m[5]; rot[7] = m[9];
	rot[2] = m[2]; rot[5] = m[6]; rot[8] = m[10];

	printf("\n rot \n %f %f %f \n %f %f %f \n %f %f %f \n ",rot[0],rot[1],rot[2],rot[3],rot[4],rot[5],rot[6],rot[7],rot[8]);
	
}

void DisplaySecondViewPort()
{
	glViewport(640,0,640,480);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();	
	gluPerspective(90,((double)640)/((double)480),0.01,10000);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	double x = _radius*sin(_latitude*M_PI/180)*sin(_azimute*M_PI/180);
	double y = _radius*cos(_latitude*M_PI/180);
	double z =  _radius*sin(_latitude*M_PI/180)*cos(_azimute*M_PI/180);
	
	gluLookAt(x,y,z,0,0,0,0,1.0,0);

	glColor4f(0.0, 0.0, 1.0, 1.0);
	glPointSize(10);
	glLineWidth(5);
	glBegin ( GL_LINES ) ;
	glColor4f(1.0, 0.0, 0.0, 1.0);
	glVertex3d ( 0, 0,0 ) ;
	glVertex3d ( 100, 0,0 ) ;
	glColor4f(0.0, 1.0, 0.0, 1.0);
	glVertex3d ( 0, 0,0 ) ;
	glVertex3d ( 0, 100,0 ) ;
	glColor4f(0.0, 0.0, 1.0, 1.0);
	glVertex3d ( 0, 0,  0 ) ;
	glVertex3d ( 0, 0,  100 ) ;
	glEnd () ;
	glLineWidth(1);

	glBegin ( GL_LINES ) ;
	glColor4f(2.0, 2.0, 1.0, 1.0);
	for (int i = 0 ; i < 50;i++) // xz
	{
		glVertex3d ( (i-25)*20,   0, -300 ) ;
		glVertex3d ( (i-25)*20,  0, 300 ) ;
	}
	glColor4f(0.0, 1.0, 1.0, 1.0);
	for (int i = 0 ; i < 20;i++) // yz
	{
		glVertex3d ( 0,(i-10)*40,    -300 ) ;
		glVertex3d ( 0,(i-10)*40,   300 ) ;
	}
	

	glEnd () ;
		
		glMultMatrixd(mvInv);
		renderAxis(30);
		glColor4f(1.0, 0.0, 0.0, 1.0);
		glutWireSphere(5.0,5,5);
		glTranslated(0,0,-100);
		glColor4f(0.0, 0.0, 1.0, 1.0);
		glutWireSphere(5.0,5,5);
		
	



}

static void DisplayPrimeiroViewPort()
{
	glClear(GL_DEPTH_BUFFER_BIT);
	glViewport(0,0,640,480);
	glMatrixMode (GL_PROJECTION);
	glLoadIdentity();
	glLoadMatrixd(proj);	
	glMatrixMode(GL_MODELVIEW);	
	glLoadIdentity();
	glColor4f(1.0,1.0,1.0,1.0);	
	glMultMatrixd(mv);
	renderAxis(30);
	renderCube(30);
}


static void Display(void)
{
	glClear( GL_COLOR_BUFFER_BIT );

	/* the curve */	

	glClear(GL_DEPTH_BUFFER_BIT);
	glViewport(0,0,640,480);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluOrtho2D (0.0, 640, 0.0,  480);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glRasterPos2i(0,0);
	

	if (image.data != NULL)
	{
		//Mat rotation, translation;
		glDrawPixels(640,480, GL_RGB, GL_UNSIGNED_BYTE, (GLvoid *)image.data);
		
		
		//;;cvct.getGLMatrix(GlProject, rotation, translation);
	//	if (translation.data != NULL)
		{
		//	glGetModelViewMatrix(rotation,translation,mv);
			cvct.getGLViewMatrix(mv);
			getCameraPosition(mv, mvInv, cameraPos, cameraRot  );

			
			DisplayPrimeiroViewPort();
			DisplaySecondViewPort();

		}
	}
	glutSwapBuffers();
}



static void Mouse( int x , int y)
{
	//if (button == GLUT_LEFT_BUTTON )
	{
		if (x > 640)
		{
			if (y > 460)
			{
				_azimute = ((x - 640.0)/640.0)*360;
			}
			else
				if (x > 1260 )
				{
					_latitude = ((480 - y) / 460.0)*180;
				}
		}
	}
	
}

static void MouseWheel(int w , int dir , int x , int y)
{
	_radius += dir*10;
	
}

static void 
	Key(unsigned char key, int x, int y)
{
	int need_redisplay = 1;

	switch (key) {
	case 27:  /* Escape key */
		exit(0);
		break;

	case 't':
		realtime = (realtime)?false:true;
		break;
	case 'l':
		frame = imread("currentImage.png");
		break;
	case 'r':
		imwrite("currentImage.png",frame);
		break;

	case 'n' :
		detecFeatureMode = NATURAL;
		break ;

	case 'c' :
		detecFeatureMode = CHESSBOARD;
		break ;

	default:
		need_redisplay = 0;
		break;
	}
	if (need_redisplay)
		glutPostRedisplay();
}


static void 
	Reshape(int width, int height)
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.0f, 0.0f, 0.0f, 0.5f);				// Black Background
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glEnable ( GL_COLOR_MATERIAL );
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	cvct.getGLProjectionMatrix(proj);
}

void Idle()
{
	Mat glProj,glMv;


	if (realtime)
	{
		cap >> frame;
	}
	
	cvtColor(frame, image, CV_BGR2RGB); 		
	cvtColor(frame, gray, CV_BGR2GRAY);		

	cvct.faztudo(prevGray,gray);
	cvct.drawReprojection(image);
	swap( prevGray, gray );
	


	if( !frame.empty() && !image.empty() )
	{
		cv::flip(image,image,0);
		glutPostRedisplay();
	}
}

int 
	main(int argc, char *argv[])
{
	int fractal_window ;


	TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03);
	Size subPixWinSize(10,10), winSize(31,31);

	const int MAX_COUNT = 500;
	bool needToInit = false;
	bool nightMode = false;

	if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0])))
		cap.open(argc == 2 ? argv[1][0] - '0' : 0);
	else if( argc == 2 )
		cap.open(argv[1]);

	if( !cap.isOpened() )
	{
		cout << "Could not initialize capturing...\n";
		return 0;
	}

#if 1
	isNaturalAvalible = cvct.loadMarkerImage("d:\\android16\\temp\\obidosRR.png");
#else
	cv::Mat imageFile = cv::imread("d:\\android16\\temp\\obidosRR.png");
	if (imageFile.data == NULL)
	{
		isNaturalAvalible =  false;
	}else
		cvct.loadMarkerImage(imageFile);
#endif


	glutInitWindowSize(1280, 480);
	glutInitWindowPosition ( 140, 140 );
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE );
	glutInit(&argc, argv);


	fractal_window = glutCreateWindow( "CVCTrackerTestGL" );

	glClearColor(1.0, 1.0, 1.0, 1.0);

	glutReshapeFunc(Reshape);
	glutIdleFunc(Idle);
	glutKeyboardFunc(Key);
	//glutMouseFunc(Mouse);
	glutMotionFunc(Mouse);
	glutMouseWheelFunc(MouseWheel);
	//glutSpecialFunc(Special);
	glutDisplayFunc(Display);

#ifdef WIN32
#endif

	//while ( true )
	glutMainLoop();

	printf ( "Back from the 'freeglut' main loop\n" ) ;

	return 0;             /* ANSI C requires main to return int. */
}

